{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test BIT* Planner on Static Rand Maze2d Env"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys; sys.path.append('..') # . is the dir that launch the program\n",
    "import time, pdb\n",
    "import numpy as np\n",
    "from pb_diff_envs.environment.static.rand_maze2d_env import RandMaze2DEnv\n",
    "\n",
    "maze_size = np.array([5, 5]) ## only support this size\n",
    "\n",
    "wall_locations = np.array(\n",
    "       [[0.77086047, 1.10158635],\n",
    "       [0.96595155, 4.01959581],\n",
    "       [1.56909837, 2.70535743],\n",
    "       [2.31427047, 0.94331186],\n",
    "       [2.77202437, 4.29066585],\n",
    "       [4.04977404, 2.85560483]]\n",
    "       )\n",
    "wall_hExts = np.array(\n",
    "    [[0.5, 0.5],[0.5, 0.5],[0.5, 0.5],\n",
    "      [0.5, 0.5],[0.5, 0.5],[0.5, 0.5]])\n",
    "\n",
    "robot_config = dict(maze_size=maze_size, min_to_wall_dist=0.01, collision_eps=0.02)\n",
    "\n",
    "env = RandMaze2DEnv(wall_locations, wall_hExts, robot_config, renderer_config={})\n",
    "env.load(GUI=True)\n",
    "prev_pose = env.get_robot_free_pose()\n",
    "solution, _ = env.sample_1_episode(prev_pose)\n",
    "print('solution', solution)\n",
    "\n",
    "env.robot.set_config(prev_pose)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Visualize the Output Traj"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils.kuka_utils_luo import SolutionInterp\n",
    "import matplotlib.pyplot as plt\n",
    "sol_interp = SolutionInterp(density=10)\n",
    "traj_vis = sol_interp(solution)\n",
    "print('solution', len(solution))\n",
    "solution_vis = np.array(solution)\n",
    "print('traj_vis', len(traj_vis))\n",
    "savepath = None\n",
    "img = env.render_1_traj(savepath, traj_vis[-70:],)\n",
    "plt.imshow(img)\n",
    "plt.show()\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Vis many trajs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils.utils import DotDict\n",
    "from tqdm import tqdm\n",
    "from utils.kuka_utils_luo import SolutionInterp\n",
    "import matplotlib.pyplot as plt\n",
    "sol_interp = SolutionInterp(density=8)\n",
    "env.load(GUI=True) # False\n",
    "prev_pose = env.get_robot_free_pose()\n",
    "env.robot.set_config(prev_pose)\n",
    "solutions = []\n",
    "# n_traj = 25\n",
    "n_traj = 5\n",
    "for i in tqdm(range(n_traj)):\n",
    "    solution, _ = env.sample_1_episode(prev_pose)\n",
    "    solution = np.array(solution)\n",
    "    prev_pose = solution[-1]\n",
    "    solutions.append(solution)\n",
    "\n",
    "trajs = solutions\n",
    "solutions = np.concatenate(solutions, axis=0)\n",
    "result = DotDict(solution=solutions)\n",
    "\n",
    "\n",
    "print(solutions.shape)\n",
    "print(len(result.solution))\n",
    "# result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "solution = np.array(result.solution)\n",
    "# env.render_1_traj(solution)\n",
    "for i in tqdm(range(n_traj)):\n",
    "    trajs[i] = sol_interp(trajs[i])\n",
    "savepath = None # './bit_star_planner_randmaze2d-2.png'\n",
    "tmp = env.render_composite(savepath, trajs)\n",
    "print(tmp.shape)\n",
    "plt.clf()\n",
    "plt.imshow(tmp)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
